#include "mklm_status_provider.h"

namespace infrastructure {
namespace vehicle {
namespace mklm {

// 静态成员初始化
quint16 MklmStatusProvider::s_serverPort = 29204;

MklmStatusProvider::MklmStatusProvider(int agvNumber,
                                       std::shared_ptr<kernel::ILogger> logger,
                                       QObject* parent)
    : QObject(parent), m_agvNumber(agvNumber), m_logger(logger) {}

MklmStatusProvider::~MklmStatusProvider() { stop(); }

kernel::Result<void> MklmStatusProvider::start() {
  if (m_isRunning) {
    return kernel::Result<void>::success();
  }

  m_logger->info(
      QString("启动 MKLM StatusProvider for AGV %1").arg(m_agvNumber));

  // 获取共享的 ServerManager
  auto* serverManager = MklmServerManager::instance();

  // 如果服务器未运行，初始化它
  if (!serverManager->isRunning()) {
    bool success = serverManager->initialize(s_serverPort, m_logger);
    if (!success) {
      m_logger->error(
          QString("AGV %1 无法初始化 MKLM Server").arg(m_agvNumber));
      return kernel::Result<void>::failure(kernel::ErrorCode::NetworkError,
                                           "Failed to initialize MKLM Server");
    }
  }

  // 注册回调到 ServerManager（接收 MklmAgvStatusDTO）
  serverManager->registerAgvCallback(
      m_agvNumber, [this](const kernel::dtos::MklmAgvStatusDTO& status) {
        this->onStatusUpdate(status);
      });

  m_isRunning = true;

  m_logger->info(QString("AGV %1 StatusProvider 已启动，已注册到共享端口 %2")
                     .arg(m_agvNumber)
                     .arg(s_serverPort));

  return kernel::Result<void>::success();
}

void MklmStatusProvider::stop() {
  if (!m_isRunning) {
    return;
  }

  m_logger->info(
      QString("停止 MKLM StatusProvider for AGV %1").arg(m_agvNumber));

  // 从 ServerManager 注销
  auto* serverManager = MklmServerManager::instance();
  serverManager->unregisterAgvCallback(m_agvNumber);

  m_isRunning = false;
}

bool MklmStatusProvider::isRunning() const { return m_isRunning; }

void MklmStatusProvider::setStatusUpdateCallback(
    StatusUpdateCallback callback) {
  m_statusCallback = callback;
}

int MklmStatusProvider::getAgvNumber() const { return m_agvNumber; }

void MklmStatusProvider::setServerPort(quint16 port) { s_serverPort = port; }

quint16 MklmStatusProvider::getServerPort() { return s_serverPort; }

void MklmStatusProvider::onStatusUpdate(
    const kernel::dtos::MklmAgvStatusDTO& mklmStatus) {
  // 将 MklmAgvStatusDTO 转换为通用的 AgvStatusDTO
  kernel::dtos::AgvStatusDTO status;

  // 基本信息
  status.agvNumber = mklmStatus.agvNumber;
  status.ip = mklmStatus.ip;

  // 位置信息
  status.mode = mklmStatus.mode;
  status.x = mklmStatus.x;
  status.y = mklmStatus.y;
  status.angle = mklmStatus.angle;
  status.confidence = mklmStatus.confidence;
  status.currentStation = mklmStatus.currentStation;
  status.lastStation = mklmStatus.lastStation;
  status.current_map = mklmStatus.currentMap;

  // 任务信息
  status.taskStatus = mklmStatus.taskStatus;
  status.taskType = mklmStatus.taskType;
  status.targetId = mklmStatus.targetId;
  status.unfinishedPath = mklmStatus.unfinishedPath;
  status.finishedPath = mklmStatus.finishedPath;

  // 阻挡信息
  status.blocked = mklmStatus.blocked;
  status.driverEmc = mklmStatus.driverEmc;

  // 区域信息
  status.areaIds = mklmStatus.areaIds;

  // 电池信息
  status.batteryLevel = mklmStatus.power; 
  status.charging = mklmStatus.chargeState;

  // 速度信息（MKLM协议不提供，设为默认值）
  status.vx = 0.0;
  status.vy = 0.0;
  status.w = 0.0;
  status.steer = 0.0;
  status.spin = 0.0;

  // 阻挡和停止状态
  status.isStop = false;  // MKLM协议不提供
  status.blockReason = 0;  // MKLM协议不提供具体原因

  // 路径信息（MKLM协议不提供JSON格式路径）
  status.path = QJsonArray();

  // 急停和传感器（MKLM协议不提供）
  status.emergency = false;
  status.driverEmc = false;
  status.digitalInputs.clear();
  status.digitalOutputs.clear();

  // 姿态信息（MKLM协议不提供）
  status.yaw = 0.0;
  status.roll = 0.0;
  status.pitch = 0.0;

  // 目标点信息（MKLM协议不提供JSON格式）
  status.targetPoint = QJsonArray();

  // 转发到用户设置的回调
  if (m_statusCallback) {
    m_statusCallback(status);
  }
}

}  // namespace mklm
}  // namespace vehicle
}  // namespace infrastructure
